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Abstract. In this paper we will discuss some new developments in the 
design of numerical methods for optimal control problems of Lagrangian 
O ^ systems on Lie groups. We will construct these geometric integrators us- 

D ■ ing discrete variational calculus on Lie groups, deriving a discrete version 

^0 ' of the second-order Euler-Lagrange equations. Interesting applications 

, as, for instance, a discrete derivation of the Euler-Poincare equations 

■ for second-order Lagrangians and its application to optimal control of a 

rigid body, and of a Cosserat rod are shown at the end of the paper. 

S 1 : 



L • 1. Introduction 

-5; 

C$ , The goal of this paper is to study, from a geometric point of view, varia- 

tional integrators for optimal control problems of mechanical systems defined 
'— on finite dimensional Lie groups, and its applications in optimal control the- 
i \ ory. Our motivation is the control of autonomous vehicles modeled as rigid 

J> ■ bodies (as an evolution equation in time). 

^^O , We use the theory of discrete mechanics based on discrete variational 

^ | calculus [25]. In particular, we use Hamilton's principle yielding the set of 

t^j- ■ discrete paths that approximately satisfy the dynamics. This is achieved by 

| formulating a second order discrete variational problem solved through dis- 

' crete Hamilton's principle on Lie groups and obtaining a variational numeric 

integrator. Such formulation gives us the preservation of important geomet- 
ric properties of the mechanical system, such as momentum, symplecticity, 
group structure, good behavior of the energy, etc [IT] . 

A typical optimal control problem consists on finding a trajectory of the 
state variables and controls (g(t),£(t),u(t)) given fixed initial and final con- 
ditions (<7(0),£(0)) and (g(T),£(T)) respectively, and, as well, minimizing 
the cost functional defined by 



J(u,T) = [ \\u{t)\\ 2 dt; 
Jo 

here, g(t) evolves on a Lie group 67, £(t) on the associated Lie algebra g and 
u(t) on the space of admissible controls. 

Our approach is based on recently developed structure-preserving numer- 
ics integrators for optimal control problems (see [8] , [9] , [16] , [IT], [20], [27] 
and references therein) based on solving a discrete optimal control problem 
as a discrete higher-order variational problem with higher-order constraints 
(see [3] for the continuous case) which are used for simulating and controlling 
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the dynamics for satellites, spacecrafts, underwater vehicles, mobile robots, 
helicopters, wheeled vehicles, mobile robots, etc [5]. 

1.1. Background: Discrete Mechanics and variational integrators. 

Let Q be a n-dimensional differentiable manifold, the configuration manifold, 
with local coordinates {q l ), 1 < i < n. Denote by TQ its tangent bundle 
with induced coordinates (q l ,q 1 )- Given a Lagrangian function L : TQ —¥ R, 
the Euler-Lagrange equations are 

d ( dL\ dL 

sis?) -a?- ' 1 - s - n - (1) 

These equations are a system of implicit second order differential equations. 
In the sequel, we will assume that the Lagrangian is regular, that is, the 

matrix ^ J is non-singular. It is well known that the origin of these 

equations is variational (see [T].[23]). 

Variational integrators |25| are derived from a discrete variational prin- 
ciple. These integrators also retain some of main geometric properties of 
the continuous system, such as simplecticity, momentum conservation and 
a good behavior of the energy associated with the Lagrangian system (see 
[llj and references therein). 

In the sequel we will review the construction of this type of geometric 
integrators. 

A discrete Lagrangian is a map L d '- Q x Q — > IR, which may be con- 
sidered as an approximation of the integral action defined by a continuous 
Lagrangian L : TQ — > IR, 

L d (q ,qi) ~ f L(q(t),q(t)) dt 



JO 

where q(t) is a solution of the Euler-Lagrange equations for L; q(0) = q$, 
q(h) = q\ and the time step h > is small enough. 

Define the action sum Ad : Q N+1 — > H, corresponding to the Lagrangian 
Ld by 

N 

•A-d = Ld(qk-i,qk), 

k=l 

where q^ € Q for < k < N, where iV is the number of steps. The discrete 
variational principle then requires that 5Ad = where the variations are 
taken with respect to each point qj-, 1 < k < N — 1 along the path, and the 
resulting equations of motion (system of difference equations) given fixed 
endpoints qo and qjy, are 

D\L d (q k , q k+ i) + D 2 L d (q k _ 1 , q k ) = 0, (2) 

where D\ and D 2 denote the derivative to the Lagrangian respect the first 
and second arguments, respectively. 

These equations are usually called discrete Euler-Lagrange equa- 
tions. Under some regularity hypotheses (the matrix (Di2Ld(q k ,q k +\)) is 
regular), it is possible to define a (local) discrete flow T i d : Q x Q — > Q x Q, 
by Y Ld (q k _i,q k ) = (?fc,?ft+i) f rom ©• 
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We introduce now the two discrete Legendre transformations associated 
to L d : 

F-L d :QxQ -> T*Q 

(Qo,Qi) ^ (Qo, -DiL d (q , qi)) , 

(3) 

F + L d :QxQ -> T*Q 

(^0,9i) ^ (qi,D 2 L d (q ,q 1 )) , 

and the discrete Poincare-Cartan 2-form u d = (F + L d )*ujQ = (F~ L d )* ljq, 
where ojq is the canonical symplectic form on T*Q. uj d is a symplectic form 
if the discrete Lagrangian is regular, which is indeed equivalent to F~L d (or 
W + L d ) being a local diffeomorphism. 

The discrete algorithm determined by preserves the (pre-) symplectic 
form on T*(Q x Q), uj d , i.e., T* L u: d = u d . Moreover, if the discrete La- 
grangian is invariant under the diagonal action of a Lie group G, then the 
discrete momentum map J d : Q x Q — >• g* defined by 

(Jd{Qk,qk+i),0 = (D2L d (q k ,q k+1 ),£ Q (q k+1 )) 

is preserved by the discrete flow. Therefore, these integrators are symplectic- 
momentum preserving. Here, £q denotes the fundamental vector field de- 
termined by £ G g, where g is the Lie algebra of G, 

^ } = Jt\tJ eXp{t ° ■ q) 
for g G Q ( see [25] for more details). 

Example 1.1. For instance we consider a Lagrangian L(q,q) = ^q T Mq — 
V(q), where q S R 3 , M being a symmetric non-degenerate matrix and V 
a potential function. From this Lagrangian we construct the discrete La- 
grangian taking an Euler's discretization, 

L d (q k ,q k+l ) = h \{^P±) T M (^*) - V(q k 
We compute -Di L d and D2 : 

D 1 L d {q k ,q k+1 ) = -M T k+l ~ qk \ -hVV(q k ), 

D 2 L d (q k ^ 1 ,q k ) = M f — ^ k ~ l 

which leads to the discrete Euler-Lagrange equations: 

M + = -VV(q k ). 

We observe that these equations give rise a natural discrete version of the 
Newton's law Mq = — VV(g), using a simple finite difference rule for the 
derivative (see [25]). 
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1.2. Organization of the paper. The paper is structured as follows. In 
Section [2] we recall some results given in [23] about Hamilton's principle on 
Lie groups and the discrete Euler-Poincare equations. The new proposed 
method appears in Section [3l First, we derive the continuous second-order 
Euler-Poincare equations on Lie groups from Hamilton's principle; next, we 
construct from a discretization of the Lagrangian and through discrete varia- 
tional calculus the discrete second-order Euler-Lagrange and Euler-Poincare 
equations. The discrete higher-order Euler-Lagrange and discrete higher or- 
der Euler-Poincare equations are derived using discrete Hamilton's principle. 
In the last section, we apply these techniques to optimal control of mechani- 
cal systems and we analyze two examples of optimal control on a rigid body 
on the Lie group SO (3) and on a Cosserat rod defined on SE(S). 

2. Discrete mechanics on Lie groups 

In this section we recall the discrete mechanics on Lie groups and Hamil- 
ton's principle on Lie groups for the formulation of Euler-Poincare equations. 

2.1. Discrete Hamilton's principle on Lie groups and Euler-Poincare 
equations. If the configuration space is a Lie group G, then the discrete tra- 
jectory is represented numerically using a set of N + l points (go,gi, ■ ■ ■ , <?at) 
with gi G G, < i < N. 

A way to discretize a continuous problem is using a retraction map r : g — >• 
G which is an analytic local diffeomorphism which maps a neighborhood of 
€ to a neighborhood of the neutral element e G G. As a consequence, it 
is possible to deduce that t(£)t(— £) = e for all £ G g. The retraction map 
is used to express small discrete changes in the group configuration through 
unique Lie algebra elements (see [E]), namely = T~ 1 (gZ gk+i)/h, where 
£fc £ 0- That is, if were regarded as an average velocity between g^ and 
gk+i, then r is an approximation to the integral flow of the dynamics. The 
difference g^ 1 gk+i S G, which is an element of a nonlinear space, can now be 
represented by the vector in order to enable unconstrained optimization 
in the linear space for optimal control purposes. 

It will be useful in the sequel, mainly in the derivation of the discrete 
equations of motion, to define the right trivialized tangent retraction map 
as 

Tgr = T e r Ti o dr ? . 

Useful and complementary definition of the right trivialized (and its inverse) 
is the following ([J3], @]): 

Proposition 2.1. Given a map r : — > G, its right trivialized tangent 
dig : — > and its inverse dr^ -1 : — > g, are such that for g = r(£) G G and 
r\ G 0, the following holds 

d^r(Ov = dr^rjT^), 

An example of retraction map is the exponential map at the identity e of 
the group G, exp e : g — > G. We recall that for a finite dimensional Lie grup, 
exp e is locally a diffeomorphism and gives rise a natural chart [23]. Then, 
there exists a neighborhood U of e G G such that exp~ x : U — > exp~ l (U) is 
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a local C°° diffeomorphism. A chart at g G G is given by ^ g = exp~ o l g -i, 
where I denote the left-translation of an element of the group. 

In general, it is not easy to work with the exponential. For instance, if we 
are considering matrix groups, the right trivialized derivative and its inverse 
are defined by infinite series 

dexp x y 
dexp" 1 y 

where Bj are the Bernoulli numbers, x, y G g and ad x y = [x,y] is the usual 
matrix bracket (see Tipically, these expressions are truncated in order 

to achieve a desired order of accuracy. 

In consequence it will be useful to use a different retraction map. More 
concretely, the Cayley map (see [TT] for further details) will provide us a 
proper framework in the examples shown below. 

The following theorem, regardless of the retraction structure locally re- 
lating G and g, gives us the relation between the discrete Euler-Lagrange 
equations and the discrete Euler-Poincare equations. 

Theorem 2.1. [?] Let G be a Lie group and L d : G x G — > M a discrete 
Lagrangian function. We suppose that L d is left-invariant over the diagonal 
action (i.e; L d (gg k , gg k +i) = L d (g k ,g k+1 ) with g G G). Letl d : G -» R be the 
restriction to the identity (that is, l d : (G x G)/G ~ G — > R, Idid^ 1 fffc+i) = 
Ld(9k,9k+l))- For a pair of points (g k ,g k+ i) G G x G, we consider W k = 
g^fffc+i (where g^ 1 = i(g k ),i : G — > G the inversion map of the Lie group 
G). Then the following assertions are equivalent: 

(1) (g k )o<k<N satisfies the discrete Euler-Lagrange equations for L d . 

(2) (g k )o<k<N extremize the discrete action 

N-l 

(9k)o<k<N !->• L d (g k ,g k+ i) 

k=0 

for all variation with initial and final fixed points. 

(3) The discrete Euler-Poincare equations 

r*wJ' d (W k ) ~ i*w k J d (Wk-i) = k = 1, . . . , N 

hold, where I and r are the left- and right-translation of the Lie group 
and 'denote the partial derivative. 

(4) (Wfc) <fc<jv_i extremize 

N-l 

(W k ) < k <N-l i-> Yl ld(W k ) 
k=0 

for all variations 5W k = —T, k W k + W k T, k+ i with So = S^v = 0; 
where T, k G g is given by T, k = g k 5g k . 



= E(JTl)! ad ^< 

3=0 U 7 

00 B 

3=0 J ' 
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3. Continuous and discrete Euler-Poincare equations for 
second order lagrangians 

In this section we derive, from a variational point of view, the discrete and 
continuous Euler-Lagrange equations for second-order Lagrangians defined 
on Lie groups: the second order Euler-Poincare equations in the continuous 
and discrete setting. 

Consider a mechanical system determined by a Lagrangian L : TG — > 
R. It is well known that the tangent bundle TG can be left-trivialized as 
TG ~Gxg, where q is the Lie algebra of a Lie group G. The motion of the 
mechanical system is described by applying the following principle 

5 f T L(g(t),at))dt = (4) 
J o 

for all variations 5£(t) of the form 5£(t) = rj(t) + [£(t),i](t)], where 77 is an 
arbitrary curve on the Lie algebra with 77(0) = = 77 (T) and 5g = grj (see 
[24j ) . This principle give rise to the Euler-Lagrange equations 

d (SL\ (SL\ SL 

where ad^r/ = [£, 77]. If the Lagrangian L is left-invariant the above equations 
are written as 

and are called the Euler-Poincare equations. 



3.1. Continuous setting. In this subsection we deduce, from a variational 
principle, the Euler-Poincare equations for Lagrangians defined on T^G ~ 
G x 2q from a left-trivization. One interesting application of this theory will 
be the optimal control of mechanical systems as we will seen in the next 
section (see [7]) 

Let L : T^G ~ G x 2q — > R be a Lagrangian function, L(g,g,g) = 
L(g,£,£) where £ = g~ l g (left-trivialization) . The problem consists on find- 
ing the critical curves of the functional 

d= f T L(g,C,i)dt 
Jo 

among all curves satisfying the boundary conditions for arbitrary variations 
Sg = 4- I e =o <?e, where, e h-> g € is a smooth curve in G such that go = g. 

We define, for any e, £ e := gj l g e - The corresponding variations 5^ induced 
by Sg are given by <5£ = 77 + [£, 77] where 77 := g~ l 5g € Q (5g = grj). Therefore 
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5 [ T L(g(t),t(t),£(t))dt 



d 



e=0 



T 

L(g t (t),^(t),Ut))dt 



o 



T 



(i \ % 6£ ) 

T ( ,dL . , ,5L ,5L d ,-.. s .\ , 

,dL . . ,5L . d 5L ...N , 

T ( ,dL . 5L d 6L d ^ n \ , 

T , / J \ / XT J XT \ > /"T 



T 











l+^)(f-sf)-M <':(f)->*=» ■ 



where we have used integration by parts and the vanishing initial and end- 
point conditions 7/(0) = T](T) = r)(0) = r)(T) = 0. Thus, the stationary 
condition 53 = implies the second-order Euler- Lagrange equations, 



dL ( d \ (5L d 6L 



that is, 



„dL d 8L d 2 8L 7 JL „ / d 8L > 
C^- - 37T7T + 37?^ + adf— -tkC — — r =0. (5) 
9 eft 5£ dt 2 * <5£ 4 V* $£, 



If the Lagrangian is invariant under an action of the Lie group, the equa- 
tions of motion are 

d 2 5L d8L 8L ,*{d5L ] 

-7-7T — r -— + ad}— - ad f — — = 0. (6) 

dt 2 dt 5£ 4 5£ 4 \dt 1 



These equations are called second order Euler- Poincare equations. 

In a recent paper [10], the authors studied invariant higher order problems 
and obtain the equations © working in a reduced Lagrangian setting on 
x g. 

3.2. Discrete setting. Now, we consider the associated discrete problem. 
The second order tangent bundle is left-trivialized as T&'G ~ G x 2q and 
then we choose its natural discretization as three copies of the Lie group (we 
recall that the prescribed discretization of a Lie algebra q is its associated Lie 
group G). Consequently, we develop the discrete Euler-Lagrange equations 
for the discrete Lagrangians defined on G x G x G = 3G. 

Let Ld : 3G ->Mbea discrete Lagrangian where G is a finite dimensional 
Lie group. As in the previous section, we define = g^Qk+i- Taking 
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variations for W k , where we denote = g^Sgk, we obtain 

SW k = -g k l 5g k g k 1 g k +i + g^Sgk+i 
= -SfcWfc + g^gk+igkli^k+i 
= -SfcWfc + WfcSfc+i, 

where g k , W k € G and € 3. 

The equations of motion are the critical paths of the discrete action 

JV-2 

^2 L d (g k ,W k ,W k+1 ) 

k=0 

with boundary conditions So = £1 = £at_i = S^r = since we are assuming 
that go, g\, gw-i and gN fixed. Therefore, after some computations we 
obtain the equations 

l^Dtfiig^Wk-i, W k ) + l* Wki D 2 L d {g k ^ W k _ x ,W k ) 
-rw k D 2 L d (g k , W k , W k+1 ) - r^ k D^L d (g k - U Wk-i, W k ) 
+lt Vki D 3 L d (g k - 2 ,W k -2,W k - 1 ) = 

These equation, together with the reconstruction equation W k = 

gj^ g k +i, are called discrete second order Euler- Lagrange equations . 

If L d is G invariant in the sense that L d (g k , W k -\, W k ) = L d (hg k , W k -\, W k ) 
for all h G G then we can define the reduced lagrangian l d : G x G — > R and 
the equations are rewritten as 

= l*w k _DMW k -i,W k ) - r* Wk DMW k ,W k+1 ) 
- r* Wk D 2 l d {W k - X ,W k ) + l* Wk _D 2 l d {W k „ 2 , W fc _i) 
and are called the discrete second-order Euler- Poincare equations. 

Remark 3.1. Is easy to extend these techniques for higher order discrete 
mechanics (see [2]). Consider a mechanical system determined by a La- 
grangian L : T^G — >■ R. It is well known that the tangent bundle T^G 
can be left-trivialized as T^G ~ G x kg, where g is the Lie algebra G. 

Now, we consider the associated discrete problem. First, we replace the 
higher order tangent bundle by (k + 1) copies of the group since the pre- 
scribed discretization of each g is the Lie group G. At this point, we develop 
the discrete Euler-Poincare equations for the discrete Lagrangians defined 
on G x kG. 

Let L d : G x kG — > R be a discrete Lagrangian where G is a finite 
dimensional Lie group. As before, denote by Wj = fi^Vt+i and Sj = g^ l 8gi. 
Taking variations over VFj we obtain 

SWi = -gr 1 5g i gr 1 g i+ i + gl l 5g i+ i 
= -T,iWi + g^gi+ig^Sgt+i 
= -SiWi + WiSi+i, 

where gi,Wi £G and Ej E g. 
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The equations of motion are the critical paths of the discrete action 

N-k 

min ^ L d(9i,W(ij +k -i)) 
i=0 

with boundary conditions Eo = . . . = Ejfc-i = 0, Etv-Ai+i = • • • = £jv 
and go,---, 9k-i and g N ^ k +i, ■ ■ ■ , 9n fixed. 
Taking variations we deduce 

N-k 

i=0 
N-k 

\piL d (g i} W {iA+k) ) (g^i) 

i=k 
fc+1 

+ ^ DjL d (gi, W( iti+k -i)) (-Ej + j_ 2 Wj+i-2 + Wj +i -2^j+i-i) 

3=2 

where we denote by Wujy = (Wi, Wj+i, . . . , Wj-i, Wj). 

Therefore, the discrete higher-order Euler- Lagrange equations on G x kG 
are given by 

= ig^DiLdigi-i, W(j_i ji+ fc_i)) 
fc+i 

+ (^_i) D j L d(gi-j+i, W(i- j+1 j-j +k )) 

fc+1 

- ( r Wi) D 3 L d(9i-j+2, W(i^ j+ 2,i-j +k+ l))- 
3=2 

where k < i < N — k. 

These equations, together with the reconstruction equation W{ = g^ l gi+\ 
are called the discrete higher-order Euler- Lagrange equations. If L d is G- 
invariant, that is L d {g i ,W( i;i+k _ 1 - ) ) = L d (hgi,W^ i+k ^i)) V7t € G, we can 
consider the reduced Lagrangian l d : kG — > R. Then the discrete higher- 
order Euler- Poincare equations on the reduced space kG are given by 

fc+i 

= { l Wi-i) D 3 L d(W(i-j+l,i-j+k)) 
3=2 
fc+1 

- Y ( r ^) I) i I '(i(W(i-i+2,i-i+fe+l))- 

i=2 

4. Discrete Optimal control problems on Lie groups 

The proposal of this section is to study optimal control problems in the 
case of fully actuated mechanical systems. The discrete approximation to 
the solutions of the system have a purely discrete variational formulation 
and as a consequence, the integrators defined in this way are symplectic 
(Poisson)-momentum preserving. By using backward error analysis, it is 
well known that these integrators have a good energy behavior (see [25j). 
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As particular examples, we will study the optimal control of the rigid 
body and the Cosserat rod. The configuration groups in these examples are 
50(3) and SE(3) respectively. Both are particular cases of quadratic Lie 
groups, which are defined as 

G = {Y € GL(n,R) | Y T PY = Y) 

where P £ GL(n,H) is a given matrix (here, GL(n, R) denotes the general 
linear group of degree n). The corresponding Lie algebra is 

= {n e gZ(n, r) | pn + np = o} . 

As mentioned in subsection 12.14 the Cayley map, defined for quadratic Lie 
groups as 

cay«)=(/-f)"(/ + f), 

where £ 6 q, also gives a useful and simpler discretization of these systems. 

4.1. Example: Rigid body. The rigid body problem is very well known 
in the literature. This setting is deeply studied in [T71 [T51 [TU] among other 
references. 

The continuous equations of motion of the controlled rigid body system 
are the following 

^(1) = Pl^(2)^(3) + n l> 

^(2) = P2^(l)^(3) + u 2, (7) 
^(3) = P3%1)%2) + u 3, 

where (f^x), 0( 2 )) ^(3)) = ^ an d (^(i)> ^(2); ^(3)) = € R 3 , u% are the 
control forces and pi € R are a redefinition of the inertia momenta of the 
problem. In the sequel we will use the typical identification of the Lie algebra 
of 50(3), so (3) with R 3 by • : R 3 ->so(3), that is if x = (x 1 ,x 2 ,x 3 ) € R 3 

/ -x 3 x 2 \ 
X = X 3 —X\ € so(3). 
\ -x 2 xi / 

Consequently x x y = — [x, y] = ad^y. With some abuse of notation, we will 
directly identify R 3 with so (3) by omitting the hat notation. 

Our fixed boundary conditions are (i?(0), f2(0)) and (R(T), Q(T)), where 
R(t) £ 50(3) is the attitude of the rigid body subject to the conditions 
R = RO, and 5R = Rrj, with r] an arbitrary element of so (3). Besides the 
equations, the cost functional is 

C = / -u T udt, 
Jo 2 

where u = (u±, u 2 ,u 3 ). From eqs. ([7j) we can work out u in terms of f2 and 
O. Consequently, we can define the function / : so (3) x so (3) — > R in the 
following way 
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Therefore, the Lagrangian function has the following form: 

If- \2 1 / . \ 2 

l(n,n) = - ^fi(i) - piO( 2 )0(3)J + - [tip) - P2^(l)fi(3)J + 

+ J («(3) -P3^(l)^(2)) 2 - (8) 



With this redefinition, the cost functional becomes 



• Discrete setting: Our goal is to find and algorithm in N steps of time 
size h, i.e. Nh = T, that both minimizes the cost functional and respects 
the boundary conditions above. In order to that, we fix a discretization 
setting 

Rk+i = Rk T~(hn k ), 5R k = R k 7] k , (9) 
where r\ k G so (3) such that r/o = VN = and r(hfl k ) € 50(3) is choosen 
to be a general retraction map. As mentioned before, the first equation 
R k +i = Rk T~(h£l k ) is called the reconstruction equation. From ©, it is easy 
to obtain the variations of the algebra elements, namely 

5fl k = dr^ k (-r] k + Ad r(M4) ?7fc + i)//i, (10) 

where Ad 5 £ = g £ <? _1 , being £ G 3 and g £ G. 

Our discretization choice enables us to work with algebra elements instead 
of group ones. Thus, we define the discrete function Id : so (3) x so (3) — > R 
like ld(£lk, f^fe+i) = hl(£l k , ^ fc+ fe ^ fc ), where Z(fi,f2) is explicitly defined in 
©. We have set the usual discretization for the derivative Cl k = ^ k+1 h ^ k ■ 
In consequence, let the discrete cost functional be 

N-l 

e d =J2Un k ,n k+1 ). (11) 

k=0 

Therefore, our original optimal control problem defined by I and the bound- 
ary conditions (12(0), 0(0)) and (R(T),Q(T)) have become a discrete La- 
grangian problem with discrete action sum (jlip . Applying the Hamilton's 
principle, taking into account the right trivialized derivative of the retraction 
map defined in (|2.1j) and considering (|10p . we obtain the discrete equations 
of motion: 

Kt^nj^i-/ (DiktJVi.nfc) + D 2 i d {£i k -2, fifc-0) 

-(dr-Jj* (Dtl^Clk, n fc+1 ) + D 2 l d (Sl k - X , n k )) = 0, 

(12) 

fc = 2,..., AT- 1, 

where Lq and -D2 represent the partial derivative w.r.t. the first and second 
variables respectively. 

• Boundary conditions: from our discretization choice R k +i = R k r(hQ k ), 
is clear that fixing £l k implies constraints in the neighboring points, in this 
case R k +\ and R k . If we allow Qjy, that means constraints at the points 
Rn and Rn+i- Since we only consider time points up to t = Nh, having a 
constraint in the beyond-terminal configuration point Rn+i makes no sense. 
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Hence, to ensure that the effect of the terminal constraint on 0, is correctely 
accounted for, the set of unknown algebra points (velocities) must be reduced 
to fio:JV-l- Moreover, we can set Oo = ^(0), which reduces again, since f2(0) 
is fixed, the unknown velocities to f2i : jv-i- 

On the other hand, the boundary condition R(T) is enforced by the re- 
lation r _1 (Rjj 1 R(T)) = 0. Recalling that r(0) = e, this last expression just 
means that Rn = R(T). Moreover, it is possible to translate it in terms of 
Ofc such that there is no need to optimize over any of the configurations Rk- 
In that sense, (fT2|) together with 

t~ 1 (T(hn N ^ 1 y 1 ...T(hn y 1 R 1 R{T)) =o, 

form a set of 3(iV — 1) equations (since dim (so(3)) = 3) for the 3(iV — 1) 
unknowns J7i : at_i. Consequently, the optimal control problem has become 
a nonlinear root finding problem. From the set of velocities f2o:JV-i an d 
boundary conditions (R(0),R(T)), we are able to reconstruct the configura- 
tion trajectory by means of the reconstruction equation Rk+i = RkT(h£lk). 

• Cayley map: the group of rigid body rotations is represented by 3 x 3 
matrices with orthonormal column vectors corresponding to the axes of a 
right-handed frame attached to the body. On the other hand, the algebra 
so (3) is the set of 3 x 3 antisymmetric matrices. A so (3) basis can be 
constructed as {ei, £2, 63}, ei € so(3), where {ei, e2, 63} is the standard basis 
for R 3 . Elements £ 6 so (3) can be identified with the vector ui € R 3 through 
£ = ui a e a , or £ = £j. Under such identification the Lie bracket coincides with 
the standard cross product, i.e., ad^ p = ui x p, for some p € R 3 . Using this 
identification and recalling the hat isomorphism : defined above, we have 

cay(ci) = h + 4+ ^ ||2 (* + y) ' ( 13 ) 

where I3 is the 3x3 identity. The linear maps dig and dr^ -1 are expressed 
as the 3x3 matrices 

dcay ^ = 4+ \\u || 2 ( 2/ 3 + ^)' dcayj 1 = / 3 - I + (14) 

4.2. Example: Cosserat rod. This example is also known as Kirchhoff 's 
rod. The Cosserat theory of rods is given in the Lagrangian setting. A static 
rod corresponds to a Lagrangian system where the energy density takes the 
role of the Lagrangian function. 

The potential energy density is the object of most importance in rod the- 
ory. This energy density function (depending on the space curve parameter) 
is equivalent to the Lagrangian function of a time-dependent mechanical 
system, such that the static equilibrium equations of a rod correspond to 
the Euler-Lagrange equations of the latter. 

In this subsection we develop a discrete theory for the Cosserat rod and 
treat the associated optimal control problem. An alternatively formulation 
of the discrete theory for the study of symmetries is given in j!5j . 

The original problem is defined on the tangent bundle of the manifold 
Q = SO (3) x E 3 = SE(3) by means of the potential energy W = W nt + 
W ext : TQ — > R. The variables of our problem are (R,r, R,r), where both 
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r,r G R 3 , R G SO (3) and R G T R SO(3). If we assume that the W int is 
frame independent then 

W int (R,r,R,r) = W int (R^ 1 R, R~ lr r) = W int (u,v), 

where u = R~ l R G so(3) and v = R r G R 3 . Therefore, our new 
problem is defined in the left-trivialized tangent space SE{3) x se(3) as 
"W = W mt (u, v) + W ext (R,r). With some abuse of notation, let define he 
elements of SE(3) and se(3) = so(3) x R 3 as 

* = (fl,r)=(* €SE(3), * = (u,v) = ( * q)g S c(3), (15) 

where O3 is the null 1x3 matrix (both $ and <f> are 4x4 matrices). Finally, 
the total potential energy is 



V= I [W int (u,v)+W ext ( y R,r)}dt. 
Jo 



The equilibrium configurations of any static system coincide with the critical 
points of the potential energy. In order to obtain the equations of motion, 
we consider the following 

6il = [u, £«] + ^^u, 8v = uT. v - Y, u v + ^^v, (16) 

where 

S u = BtHR G so(3), £„ = irtr G R 3 (17) 

are independent and satisfy the boundary conditions £ u (0) = S U (T) = 
X„(0) = S„(T) = 0. It is easy to imagine that both elements form a point 
in se(3), namely 

o 3 

Taking variations of V, considering equations (|16|) and the redefinition 

dW int (u,v) dW int (u,v) , . 

and 

_ 9W ea; * (i?, r) _ dW ext {R, r) 

f ~ Ft l ~ 8R ' (19) 

which we consider the control forces, we finally arrive to the equations of 
motion 

n + nxu + / = 0, 

m + nxv + mxu + l = 0. (20) 

For more details see |15| 

The optimal control problem consists on finding a trajectory of the state 
variables and control inputs that minimize the cost functional 

l-T 



e= / (/ 2 + p\i 2 ) dt, 

Jo 



where p\ is a weight constant. The control problem is subject to the following 
boundary conditions $(0) = (R(0), r(0)), 0(0) = (tt(0),w(0)) and $(T) = 
(R(T),r(T)), <P(T) = (u(T),v(T)) belonging to SE{3) x sc(3). 
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As in the rigid body example, from eqs. (|20p we can obtain an expression 
of / and I in terms of the other variables. Furthermore, differentiating 
equations (|18p with respect to time, we can find out n and m in terms of 



((u,v), (ii,v)) if we assume W mt (u, v) twice differentiable, i.e., 

"K(u,v) f ^ J, where 'K is the Hessian matrix of W mt (u,v). Now, setting 

the function L : se(3) x se(3) — > H as L((u, v), (u, v)) = [/((it, v), (u, v))] 2 + 
p\ [l((u, v), (u, v))] 2 , our problem reduces to extremize the control functional 

G= f L((u,v),(u,v))dt = [ L(<f>,4>)dt, (21) 
Jo Jo 

subject to the boundary conditions above. For sake of completeness we can 
write down the explicit form of L, namely 

L((u,v),(u,v)) = f((u,v),(u,v)) 2 + p 2 1 l((u,v),{u,v)) 2 = 

(Ji 11 (u,v)u + 3i 12 (u,v)v + d v W int (u,v) x uf + 

+Px(0<2i(u,v) u + 3i22(u,v) v + 

+d v W int {u,v) xv + d u W m \u,v) x uf. 

• Discrete Setting: again we look for an algorithm minimizing the cost 
functional ([2"T]) and subject to the boundary conditions. Firstly, we define 
the discrete Lagrangian function : se(3) x se(3) — > M, as 

Ld(4>k, <Pk+i) = hL ((j> k , ^ k+1 h ~ 
and then the discrete cost functional 

N-l 

Q d =Y J L d^kAk+i)- (22) 

k=0 

From now on, our discussion is equivalent to the rigid body example devel- 
oped in (|4.ip . We fix the discretization setting 

= $ k r(h(/> k ), 5 $ fc = $fcS fc , (23) 

where S se(3) s.t. So = Sat = and r : se(3) — >■ SE(3) is a general 
retraction map. Consequently, the variations of 4> k are 

54> k = dr^ k (-E k + Ad r( ^)E fc+ i)//t, 

and the discrete equations of motion: 

A ^*-i)( dT /S k -i)* (DiLdifa-ufa) + D 2 L d (4> k ^ <f> k _i)) 

-( dr W k Y {DiL d ((l>k, 0fc+i) + D 2 L d {<j) k - U <j) k )) = 0, 



k = 2,...,N - 1, 

• Boundary conditions: our reconstruction equation <& k +i = ^k T {h<p k ) 
and boundary conditions (<&(0), </>(0)), (<&(0), </>(0)) reduce our set of un- 
knowns to 0i:j\r— i- The discrete equations of motion together with the 
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boundary condition <J>(T) = $at enforced by the equation 

r- 1 (rC^jv-i)- 1 ..^^)- 1 ^ 1 *^) = 0, 
where <5 _1 is given by 

_i ( R- 1 -R- l r 



3 1 

form a set of 6(iV — 1) equations for the 6(iV — 1) unknowns 0i : jv— l (since 
dim(se(3)) = 6). Again, the optimal control problem has become a nonlinear 
root finding problem. 

• Cayley map: considering the elements of SE(3) and se(3) defined in 
(fl"5j) . the Cayley transform cay : se(3) — > SE(3) is defined by 

c&m = f^yso { 3)(u) d ca y u v\ (24) 

where cay^Q^ : so(3) — > SO (3) is given by (fT3|) and dcay : R 3 —¥ R 3 by 

OH). 



4.2.1. A direct computation. Choosing r = cay in (|23p and taking into ac- 
count ([24"]) . the reconstruction equation $> k+ \ = ^kcay(h(j) k ) splits as follows: 

Rk+i = -Rfccay 50 ( 3) (M fc ), r fc+ i = r fc + hR k dc&y hUk (v k ). 

For sake of simplicity, we take a truncation of the second equation such that 
the reconstruction setting stands as 

R k+ i = R k cay so ^(hu k ), r k+1 = r k + hR k v k . (25) 



The second equation in (|25p clearly represents the easiest discretization of 
the frame independence condition v = R~ 1 r, which in our opinion makes the 
truncation non-trivial. In order to complete the discrete setting, we define 
g k = cay so (3) (hu k ) and the variations of the SE(3) elements as 

5R k = R k (^ u ) k , 5r k = R k (E v ) k , (26) 

such that (£ u ) = (Y, U ) N = 3x3 , (E„) = = 0. 

By means of (|25p and (|26p we can completely determine 5u k and 5v k in 
terms of u fc , v k , (T, u ) k and (£„) fe : 



h 



h h 
Ad 9fc (£„)A; + i - (S u ) fc + -ad„ fc (S„) fc - -ad^ fe Ad 9fe (E u ) k 



h 2 h 2 
+^-u k {Y, u ) k u k - —u k (Ad gk (Y, u ) k+1 )u k 

8v k = -(T, u ) k v k + — g k (H v ) k+ i — — (E„)fc. 
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Taking variations of in (|22p and after long calculations, we arrive to the 
following algorithm: 

a j* T 50(3) _ T 50(3) 
u Sft-i (k-2,fc-l,fc) (fe-l,fe,fc+l) ~ r 

+ ^H* T SO{3) --AH* aH* T 50{3) + 
fifc {k-l,k,k+l) 2 ffe- 1 Sfc-i (fc-2,Jfe-l,Jfe) ~r 

. ^ ~*^SO(3) ~* ^ « ,* ~* ™SO(3) ~* 

+ X fc (ik-l,fc,fe+l) U * ~~ "J 1 (k-2,k-l,k) U k-l + 



-/l[T 



[Tf fe 3 _ 1)fcjfe+1) , «*] = 0, (27) 

5fc-l 1 (fc-2,fc-l,fc) ~~ 1 (fc-l,fc,fc+l) — U ' ' JV ~~ z - 



i? fc+ i = i?fccay 50 ( 3 )(/in fc ), fc = 0, iV - 1 

(28) 

n+i = r k + hR k v k , k = 0, ...,N - 1. 

Here T 50 ® G so* (3) and T r3 G R 3 , stands for 

T fa°fe,c) := D i L d(ub, Vb, u c , v c ) + D 3 L d (u a , v a , u b , v b ), 
T ^fe,c) := D 2 L d (u b , v b , u c , v c ) + D A L d (u a , v a , u b , v b ), 

being (a, b, c) integers from 2 to N — 2. Both operators Ad* and ad* act over 
elements of so (3)*. The dual algebra element £*u>£* G so (3)* is defined such 
that (£*w£*,7/) = (u,£r]£) for uj G so(3)*, £,rj G so(3) and (•,•} the natural 
pairing between so (3) and so (3)*. 

Finally, we have obtained an algorithm that approximates in an implicit 
and non linear way the solution of the optimal control problem for the 
Cosserat rod setting. 



5. Conclusions and Future works 

5.1. Conclusions. In this paper, we have designed new variational integra- 
tors for optimal control of mechanical systems showing how developments in 
the theory of discrete mechanics and variational methods [25] can be used 
to construct numerical optimal control algorithms with certain desirable 
features. The methods are available for developing integrators on higher- 
order problems. The main idea is to use discrete variational calculus on 
Lie groups using the Lagrange-d'Alembert principle and to derive the dis- 
crete Euler-Poincare equation for discrete Lagrangians corresponding to a 
discretization of the second order Lagrangian defined on the trivialized space 
(left-trivialized) G x 2g. 

It is also possible to use our techniques and the numeric integrator ob- 
tained in this paper for other interesting problems, like for instance the 
theory of fc-splines on SO(3) [10], [26]. In this paper, we show two applica- 
tions of second-order mechanics on the Lie groups on SO (3) and SE(3), the 
rigid body and the Cosserat rod, respectively. 
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5.2. Future Work. A complete study of symmetry reduction, discrete hamil- 
tonian description, preservation of geometric structure and numerical sim- 
ulations will be developed in a future paper. This discrete approach will 
be studied and adapted to the Lie groupoid setting [6], [T3], [21]. One in- 
teresting point, for future work, is to extend our methods to underactuated 
constraints systems using discrete constrained variational calculus (see [7] 
for the continuous counterpart). The case of optimal control problems for 
mechanical systems with nonholonomic constraints will be also studied using 
some of the ideas exposed along the paper [12] . 
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